#include <pcl/registration/icp.h>
#include <pcl/common/transforms.h>
#include "publish_pointcloud.hpp" 
// 定义点云类型 
using PointT = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<PointT>;
 
void computeTransform(PointCloud::Ptr source, 
                     PointCloud::Ptr target,
                     float& dx, float& dy, 
                     float& theta_z) {
  // 创建ICP实例 
  pcl::IterativeClosestPoint<PointT, PointT> icp;
  icp.setInputSource(source); 
  icp.setInputTarget(target); 
  
  // 设置ICP参数 
  icp.setMaximumIterations(100);   // 最大迭代次数 
  icp.setTransformationEpsilon(1e-8);  // 收敛精度 
  icp.setMaxCorrespondenceDistance(0.05);  // 最大对应点距离 
  
  // 执行配准 
  PointCloud final_cloud;
  icp.align(final_cloud); 
  
  // 获取变换矩阵 
  Eigen::Matrix4f transform = icp.getFinalTransformation(); 
  
  // 提取平移分量 
  dx = transform(0,3);  // X轴平移 
  dy = transform(1,3);  // Y轴平移 
  
  // 计算Z轴旋转角度（弧度转角度）
  theta_z = atan2(transform(1,0), transform(0,0)) * 180.0 / M_PI;
}